/*
 * compute the SPHARM coefficients and the associated Mesh
 *
 * author:  Martin Styner
 *
 */

#ifdef _MSC_VER
#pragma warning ( disable : 4786 )
#pragma warning ( disable : 4503 )
#pragma warning ( disable: 4284 )
#endif

#include <time.h> // by feilang
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>

#include <itkMeshSpatialObject.h>
#include <itkMesh.h>
#include <itkSpatialObjectWriter.h>
#include <itkSpatialObjectReader.h>
#include <itkTriangleCell.h>
#include <itkDefaultDynamicMeshTraits.h>
#include <itkProcessObject.h>

#include "ParametricMeshToSPHARMSpatialObjectFilter.h"
#include "SphericalHarmonicSpatialObject.h"
#include "SphericalHarmonicMeshSource.h"
#include "SphericalHarmonicCoefficientFileWriter.h"
#include "SphericalHarmonicCoefficientFileReader.h"
#include "itkMesh3DProcrustesAlignFilter.h"

#include "vtkPolyDataToitkMesh.h"
#include "itkMeshTovtkPolyData.h"
#include "vtkPolyDataReader.h"
#include "vtkPolyDataWriter.h"
#include "vtkIterativeClosestPointTransform.h"
#include "vtkLandmarkTransform.h"
#include "vtkPointLocator.h"
#include <itkAffineTransform.h>
#include <itkTransformFileWriter.h>
#include "vtkDoubleArray.h"
#include <vtkPointData.h>
#include <vtkSmartPointer.h>
#include <vtkCenterOfMass.h>

typedef double Point3[3];
using namespace std;

int main( int argc, const char * * argv )
{
	int spharmDegree = 12;  // In our experience, the SPHARM maximal degree is should be chosen between 5 to 50. If the degree is chosen too high the reconstructed SPHARM surface will often show signs of voxelization. (default: 12)
    std::string inParaFile; // input para mesh dataset
    std::string inSurfFile; // input surface mesh dataset
    std::string outSurfFile; // input surface mesh dataset
    std::string bullseyeFile; // output regional curvature in bull's-eye format
	std::string outbase = "./";	// output Directory and base filename
	int x1Position = 0, y1Position = 0, x2Position = 0, y2Position = 0, x3Position = 0, y3Position = 0;
	bool refBasalPoint1=false, refBasalPoint3=false;
	float inSliceSpacing = 0;



    if (argc < 9) { // Check the value of argc. If not enough parameters have been passed, inform user and exit.
  		std::cout
//  		<< "Usage is  --degree <int>  --inParaFile <std::string>  --inSurfFile <std::string>  --outDir <std::string>  --outSurfFile <std::string> \n";
  		<< "Usage is  --degree <int>  --inParaFile <std::string>  --inSurfFile <std::string>  --outSurfFile <std::string>  --outBullseyeFile <std::string>\n";
  		exit(0);
  	} else { // if we got enough parameters...
  		for (int i = 1; i < argc; i++) { /* We will iterate over argv[] to get the parameters stored inside.
  		 * Note that we're starting on 1 because we don't need to know the
  		 * path of the program, which is stored in argv[0] */

  			if ((i + 1) != argc) // Check that we haven't finished parsing already
  			{
  				std::string argString = argv[i];
  				if (argString == "--degree") {
  					spharmDegree = atoi(argv[i + 1]);
  					printf("\n spharmDegree = %d", spharmDegree);
  				} else if (argString == "--inParaFile") {
  					inParaFile = argv[i + 1];
  					std::cout << "\n --inParaFile " << inParaFile ;
  				} else if (argString == "--inSurfFile") {
  					inSurfFile = argv[i + 1];
  					std::cout << "\n --inSurfFile " << inSurfFile ;
  				} else if (argString == "--outSurfFile") {
  					outSurfFile = argv[i + 1];
  					std::cout << "\n --outSurfFile " << outSurfFile ;
  				} else if (argString == "--outBullseyeFile") {
  					bullseyeFile = argv[i + 1];
  					std::cout << "\n --outBullseyeFile " << bullseyeFile;
  				} else if (argString == "--inSliceSpacing") {
  					inSliceSpacing = atof(argv[i + 1]);
  					std::cout << "\n --inSliceSpacing [" << inSliceSpacing << "]";
  				} else if (argString == "--refBasalPoint1") {
  					refBasalPoint1 = true;
  					x1Position = atoi(argv[i + 1]);
  					y1Position = atoi(argv[i + 2]);
  					std::cout << "\n --refBasalPoint1: [" << x1Position<<", "<<y1Position << "]\n \n";
  				} else if (argString == "--refBasalPoint3") {
  					refBasalPoint3 = true;
  					x1Position = atoi(argv[i + 1]);
  					y1Position = atoi(argv[i + 2]);
  					x2Position = atoi(argv[i + 3]);
  					y2Position = atoi(argv[i + 4]);
  					x3Position = atoi(argv[i + 5]);
  					y3Position = atoi(argv[i + 6]);
  					std::cout << "\n --refBasalPoint3: [" << x1Position<<", "<<y1Position << "], [" << x2Position<<", "<<y2Position << "], ["<< x3Position<<", "<<y3Position << "]\n \n";
  				}

//  				else if (argString == "--outDir")
//  				{
//					outbase = argv[i + 1];
//					std::cout << "\n --outDir " << outbase << "\n \n";
//  				}

  			}
  		}
  	}


        int subdivLevel = 20; // Subdivision level for linear ikosahedron subdivision (default: 20)
        bool flipTemplateFileOn = false; // Use the Fliptemplate for normalization (default: 0)
	    std::string flipTemplateFile;   // not necessary when flipTemplateFileOn being set to false
	    bool regTemplateFileOn = false; // Use the registration template (default: 0)
	    std::string regTemplateFile;   // not necessary when regTemplateFileOn being set to false
	    bool writePara = false; //The spherical icosahedron subdivision, as well as local phi and theta attribute files for the quality control visualization with KWMeshVisu will be written out. (default: 0)
	    bool NoParaAlignFlag = false; // Do not do any aligment/correspondence of the parametrization using the first order ellipsoid (default: 0)
	    bool debug = false; // Verbose mode (default: 0)
	    bool regParaTemplateFileOn = false; // Use the registration template (default: 0)
	    std::string regParaTemplateFile;   // not necessary when regParaTemplateFileOn being set to false
	    bool regParaPointsOn = false; // Use regParaPoints File (default: 0);
	    std::string regParaPoints;   // not necessary when regParaPointsOn being set to false
	    bool procrustesTransformationOutputOn = false; // Enables the output of the computed Procrustes affine transformation matrix. This can be used to check if improper flips occured. (default: 0)
	    std::string procrustesTransformationFile;     // not necessary when procrustesTransformationOutputOn being set to false
	    int finalFlipIndex = 0; // "This is rarely necessary,only in case the general first order ellipsoid heuristic fails...        Flipping of the parametrization ALONG a given axis will lead to a flip of the coordinates ALONG the same axis in the ellipse aligned mesh.       1 = flip along axes of x & y,     2 = flip along y & z,       3 = flip along x & z       4 = flip along x,        5 = flip along y,        6 = flip along x & y & z,       7 = flip along z  where y is the smallest, x is the second smallest and z is the long axis of the ellipsoid (default: 0)";
	    bool meshWithCurvature = false;


#define dimension 3

  typedef itk::DefaultDynamicMeshTraits<float, dimension, dimension, double> MeshTrait;
  typedef itk::Mesh<float, dimension, MeshTrait>                             MeshType;

  /** Hold on to the type information specified by the template parameters. */
  typedef  MeshType::Pointer    MeshPointer;
  typedef  MeshTrait::PointType MeshPointType;
  typedef  MeshTrait::PixelType MeshPixelType;

  /** Some convenient typedefs. */
  typedef  MeshType::Pointer                MeshPointer;
  typedef  MeshType::CellTraits             CellTraits;
  typedef  MeshType::PointsContainerPointer PointsContainerPointer;
  typedef  MeshType::PointsContainer        PointsContainer;
  typedef  MeshType::CellsContainerPointer  CellsContainerPointer;
  typedef  MeshType::CellsContainer         CellsContainer;
  typedef  MeshType::PointType              PointType;
  typedef  MeshType::CellType               CellType;
  typedef  itk::TriangleCell<CellType>      TriangleType;

  typedef itk::MeshSpatialObject<MeshType>                     MeshSpatialObjectType;
  typedef itk::SpatialObjectWriter<3, float, MeshTrait>        MeshWriterType;
  typedef neurolib::ParametricMeshToSPHARMSpatialObjectFilter  SPHARMFilterType;
  typedef itk::Mesh3DProcrustesAlignFilter<MeshType, MeshType> ProcrustesFilterType;
  typedef itk::AffineTransform<double, dimension>              TransformType;

  std::string  outFileName("dummy");
  const char * base_string;
  base_string = outbase.c_str();


  vtkPolyDataReader *VTKreader = vtkPolyDataReader::New();
  vtkPolyData *      convSurfaceMesh = vtkPolyData::New();

  MeshType::Pointer surfaceMesh = MeshType::New();
  MeshType::Pointer paraMesh = MeshType::New();
  MeshPointer       regParaTemplateMesh;
  MeshPointer       newParaMesh = MeshType::New();
//   typedef itk::SpatialObjectReader<3,float,MeshTrait> ReaderType;
//   ReaderType::Pointer readerSH = ReaderType::New();
  neurolib::SphericalHarmonicSpatialObject::Pointer flipTemplateSO = neurolib::SphericalHarmonicSpatialObject::New();

  try
    {
    VTKreader->SetFileName(inSurfFile.c_str() );
    VTKreader->Update();
    convSurfaceMesh = VTKreader->GetOutput();

    vtkPolyDataToitkMesh* VTKITKConverter = new vtkPolyDataToitkMesh;
    VTKITKConverter->SetInput(VTKreader->GetOutput() );
    surfaceMesh = VTKITKConverter->GetOutput(); // Surface Mesh Loaded
//     cout<<"test3"<<endl;
// delete (VTKITKConverter);
//     cout<<"test9"<<endl;
    }
  catch( itk::ExceptionObject ex )
    {
    std::cout << ex.GetDescription() << std::endl;
    return 1;
    }

  try
    {
    VTKreader->SetFileName(inParaFile.c_str() );
    VTKreader->Update();

    vtkPolyDataToitkMesh* VTKITKConverter2 = new vtkPolyDataToitkMesh;
    VTKITKConverter2->SetInput(VTKreader->GetOutput() );
    paraMesh = VTKITKConverter2->GetOutput(); // Parameterization Mesh Loaded
    // delete (VTKITKConverter2);
    }
  catch( itk::ExceptionObject ex )
    {
    std::cout << ex.GetDescription() << std::endl;
    return 1;
    }

  if( flipTemplateFileOn )
    {
    if( debug )
      {
      std::cout << flipTemplateFile.c_str() << endl;
      }

    try
      {
      typedef neurolib::SphericalHarmonicCoefficientFileReader CoefReaderType;
      CoefReaderType::Pointer                                coefreader = CoefReaderType::New();
      neurolib::SphericalHarmonicSpatialObject::CoefListType coeflist;

      coefreader->SetFileName(flipTemplateFile.c_str() );
      coefreader->Update();
      coefreader->GetOutput(coeflist);

      flipTemplateSO->SetCoefs(coeflist);
      }
    catch( itk::ExceptionObject ex )
      {
      std::cout << ex.GetDescription() << std::endl;
      return 1;
      }
    }
  if( debug )
    {
    std::cout << "Preprocessing done" << std::endl;
    }
  try
    {
    if( regParaTemplateFileOn )
      {
      try
        {
        if( debug )
          {
          std::cout << "Reading in regTemplateMesh" << std::endl;
          }

        VTKreader->SetFileName(regParaTemplateFile.c_str() );

        VTKreader->Update();

        vtkPolyData *convTemplateMesh = vtkPolyData::New();
        convTemplateMesh = VTKreader->GetOutput();

        vtkPolyDataToitkMesh* VTKITKConverter4 = new vtkPolyDataToitkMesh;
        VTKITKConverter4->SetInput(convTemplateMesh);
        regParaTemplateMesh = VTKITKConverter4->GetOutput();
        regParaTemplateMesh->Update();

        delete (VTKITKConverter4);

        // 2. use ICP VTK filter -> T
        vtkIterativeClosestPointTransform * ICPTransform = vtkIterativeClosestPointTransform::New();
        vtkLandmarkTransform *              LT = ICPTransform->GetLandmarkTransform();
        LT->SetModeToRigidBody();
        ICPTransform->SetSource(convTemplateMesh);
        ICPTransform->SetTarget(convSurfaceMesh);
        ICPTransform->SetMaximumNumberOfIterations(50);

        ICPTransform->Update();

        // 3. get points on surface closest to T*selected points from regTemplate
        double transPoints[3][3];
        if( regParaPointsOn )
          {
          if( debug )
            {
            std::cout << "Reading in regParaPoints" << std::endl;
            }
          std::string   line;
          long int      index;
          char          colon;
          double        points[3][3];
          std::ifstream ptReader;
          ptReader.open(regParaPoints.c_str(), std::ios::in);
          int    pointnum;
          int    i;
          double x, y, z;
          if( ptReader.fail() )
            {
            std::cout << "Error reading regParaPoints file" << std::endl;
            return 1;
            }
          if( ptReader.is_open() )
            {
            std::getline(ptReader, line);
            pointnum = 0;
            while( !ptReader.eof() && pointnum < 3 )
              {
              ptReader >> index >> colon >> x >> y >> z;
              points[pointnum][0] = x;
              points[pointnum][1] = y;
              points[pointnum][2] = z;
              pointnum++;
              }

            if( pointnum < 3 )
              {
              std::cout << "Not enough points" << std::endl;
              return 1;
              }
            ptReader.close();
            }
          // Transform points
          for( i = 0; i < 3; i++ )
            {
            ICPTransform->TransformPoint(points[i], transPoints[i]);
            }
          }
        else
          {
          std::cout << "Need regParaPoints along with regParaTemplate" << std::endl;
          return 1;
          }
        // 4. compute and apply rotation of parameter
        if( debug )
          {
          std::cout << "Calculating parameter rotation" << std::endl;
          }
        vtkPointLocator * ptLoc = vtkPointLocator::New();
        ptLoc->SetDataSet(convSurfaceMesh);
        // Find the id of the transformed points
        long int id[3];
        id[0] = ptLoc->FindClosestPoint(transPoints[0]);
        id[1] = ptLoc->FindClosestPoint(transPoints[1]);
        id[2] = ptLoc->FindClosestPoint(transPoints[2]);

        // Get the parametrization for the points
        PointsContainerPointer paraPts = paraMesh->GetPoints();
        PointType              pts[3] = {paraPts->GetElement(id[0]), paraPts->GetElement(id[1]), paraPts->GetElement(id[2])};

        // Calculate the theta and phi for the points
        double thetaPts[3], phiPts[3];
        for( int i = 0; i < 3; i++ )
          {
          thetaPts[i] = acos(pts[i][2]);           // 0 .. M_PI
          phiPts[i] = atan2(pts[i][1], pts[i][0]); // -M_PI ... M_PI
          if( phiPts[i] < 0 )
            {
            phiPts[i] = phiPts[i] + 2 * M_PI;                 // phiPts: 0...2*M_PI
            }
          }

        /*Compute the rotation so that
        a) The north pole is aligned to pts[0]
        (theta,phi) = (thetaPts[0],phiPts[0]) maps to (0,0)
        Rotation matrix given by Ry(-thetaPts[0])*Rz(-phiPts[0])
        where Rz(phi) and Ry(theta) are rotations about the z and y
        axes by angles phi and theta respectively.
        Foley, vanDam, et. al. 2nd ed. in C, page 215.

        b) phi for pts[1] after alignment is 0 or Pi
        c) The longitude for pts[2] is between 0 and Pi.

        */
        TransformType::Pointer    rotMatPhi = TransformType::New();
        TransformType::Pointer    rotMatTheta = TransformType::New();
        TransformType::Pointer    rotMat = TransformType::New();
        TransformType::MatrixType matrixPhi, matrixTheta, matrix;

        double phiRot = -phiPts[0];

        matrixPhi[0][0] = cos(phiRot);
        matrixPhi[0][1] = -sin(phiRot);
        matrixPhi[0][2] = 0;
        matrixPhi[1][0] = sin(phiRot);
        matrixPhi[1][1] = cos(phiRot);
        matrixPhi[1][2] = 0;
        matrixPhi[2][0] = 0;
        matrixPhi[2][1] = 0;
        matrixPhi[2][2] = 1;

        rotMatPhi->SetMatrix(matrixPhi);

        double thetaRot = -thetaPts[0];

        matrixTheta[0][0] = cos(thetaRot);
        matrixTheta[0][1] = 0;
        matrixTheta[0][2] = sin(thetaRot);
        matrixTheta[1][0] = 0;
        matrixTheta[1][1] = 1;
        matrixTheta[1][2] = 0;
        matrixTheta[2][0] = -sin(thetaRot);
        matrixTheta[2][1] = 0;
        matrixTheta[2][2] = cos(thetaRot);

        rotMatTheta->SetMatrix(matrixTheta);

        rotMatPhi->Compose(rotMatTheta);
        rotMat = rotMatPhi;
        matrix = rotMat->GetMatrix();

        // Calculate where pts map to
        PointType rotPts[3];
        rotPts[0] =  rotMat->TransformPoint(pts[0]);
        rotPts[1] =  rotMat->TransformPoint(pts[1]);
        rotPts[2] =  rotMat->TransformPoint(pts[2]);

        double rotTheta[3], rotPhi[3];
        for( int i = 0; i < 3; i++ )
          {
          if( rotPts[i][2] > 1 )
            {
            rotPts[i][2] = 1;
            }
          if( rotPts[i][2] < -1 )
            {
            rotPts[i][2] = -1;
            }
          rotTheta[i] = acos(rotPts[i][2]); // 0 .. M_PI
          rotPhi[i] = atan2(rotPts[i][1], rotPts[i][0]);
          if( rotPhi[i] < 0 )
            {
            rotPhi[i] = rotPhi[i] + 2 * M_PI;
            }
          }

        int    phiFlag = 0;
        double testPhi3 = rotPhi[2] - rotPhi[1];
        if( testPhi3 < 0 )
          {
          testPhi3 = testPhi3 + 2 * M_PI;
          }
        if( testPhi3 > M_PI )
          {
          phiFlag = 1;
          }

        PointsContainerPointer paraPoints = paraMesh->GetPoints();
        int                    nvert = paraPoints->Size();
        for( int i = 0; i < nvert; i++ )
          {
          PointType curPoint =  paraPoints->GetElement(i);
          PointType rotPoint = rotMat->TransformPoint(curPoint);
          if( rotPoint[2] > 1 )
            {
            rotPoint[2] = 1;
            }
          if( rotPoint[2] < -1 )
            {
            rotPoint[2] = -1;
            }
          double theta = acos(rotPoint[2]);
          double phi = atan2(rotPoint[1], rotPoint[0]);
          if( phi < 0 )
            {
            phi = phi + 2 * M_PI;
            }
          double phiNew = phi - rotPhi[1] + (phiFlag * M_PI);
          if( phiNew < 0 )
            {
            phiNew =  phiNew + 2 * M_PI;
            }
          if( phiNew >= 2 * M_PI )
            {
            phiNew = phiNew - 2 * M_PI;
            }

          PointType newPoint;
          newPoint[0] = sin(theta) * cos(phiNew);
          newPoint[1] = sin(theta) * sin(phiNew);
          newPoint[2] = rotPoint[2];
          paraPoints->SetElement(i, newPoint);
          }

        PointType testPts[3] = {paraPts->GetElement(id[0]), paraPts->GetElement(id[1]), paraPts->GetElement(id[2])};
        double    testTheta[3], testPhi[3];

        if( debug )
          {
          std::cout << "Final param.s of template points: " << std::endl;
          }
        for( int i = 0; i < 3; i++ )
          {
          testTheta[i] = acos(testPts[i][2]);  // 0 .. M_PI
          testPhi[i] = atan2(testPts[i][1], testPts[i][0]);
          if( testPhi[i] < 0 )
            {
            testPhi[i] = testPhi[i] + 2 * M_PI;
            }
          if( debug )
            {
            std::cout << "ThetaPhi[" << i << "] = (" << testTheta[i] << "," << testPhi[i] << "); ";
            }
          }
        if( debug )
          {
          std::cout << std::endl;
          }

        // 5. use that parametrization
        CellsContainerPointer paraCells = paraMesh->GetCells();
        newParaMesh->SetPoints(paraPoints);
        newParaMesh->SetCells(paraCells);
        MeshSpatialObjectType::Pointer meshSO = MeshSpatialObjectType::New();
        meshSO->SetMesh(newParaMesh);

        // if( writeFlipRegPara)
        //         {
        //           if(debug) std::cout << "saving regPara parametrization" << std::endl;
        //           MeshWriterType::Pointer metaWriter =  MeshWriterType::New();
        //           metaWriter->SetInput(meshSO);
        //           outFileName.erase();
        //           outFileName.append(base_string);
        //           outFileName.append("_regPara_para.meta");
        //           metaWriter->SetFileName(outFileName.c_str());
        //           metaWriter->Update();
        //         }

        // 7. in if regTemplate, add section for if regParaTemplate and use S2 for procrustes alignment
        }
      catch( itk::ExceptionObject ex )
        {
        std::cout << ex.GetDescription() << "while doing regParaTemplateFile" << std::endl;
        return 1;
        }
      }

    // Main body here
    SPHARMFilterType::Pointer spharmFilter = SPHARMFilterType::New();
    spharmFilter->SetInputSurfaceMesh(surfaceMesh);

    spharmFilter->SetInputParametrizationMesh(paraMesh);// Not necessary

    if( regParaTemplateFileOn )
      {
      spharmFilter->SetInputParametrizationMesh(newParaMesh);
      }
    else
      {
      spharmFilter->SetInputParametrizationMesh(paraMesh);
      }

    spharmFilter->SetDegree(spharmDegree);
    if( NoParaAlignFlag || regParaTemplateFileOn )
      {
      spharmFilter->SetParaEllipseAlignment(false);
      }
    else
      {
      spharmFilter->SetParaEllipseAlignment(true);
      }
    if( flipTemplateFileOn && !regParaTemplateFileOn )
      {
      spharmFilter->SetFlipTemplate(flipTemplateSO);
      }
    if( finalFlipIndex > 0 && finalFlipIndex <= 7 )
      {
      spharmFilter->SetParametrizationFlipIndex(finalFlipIndex);
      }

    clock_t begin, end, start, finish;// by feilang
    time(&begin);
    start = clock();
    spharmFilter->GenerateData(); // **Coefficient Computation
    // spharmFilter->Update();
    time(&end);
    finish = clock();
    std::cout << "GenerateData--Coefficient Computation:" << difftime(end,begin) << std::endl;
    std::cout << "GenerateData--Coefficient Computation:" << finish-start << "--CLOCKS_PER_SEC:" <<CLOCKS_PER_SEC << std::endl;

    if( debug )
      {
      std::cout << "saving par aligned data" << std::endl;
      }

    neurolib::SphericalHarmonicSpatialObject::Pointer      spharmSO = spharmFilter->GetOutput();
    neurolib::SphericalHarmonicSpatialObject::CoefListType coeflist;
    spharmSO->GetCoefs(coeflist); // coefficients stored in coeflist

    // save associated surface
    neurolib::SphericalHarmonicMeshSource::Pointer meshsrc = neurolib::SphericalHarmonicMeshSource::New();
    meshsrc->SetCoefs(coeflist);
    meshsrc->SetLevel(subdivLevel);
    meshsrc->Update();

    MeshType* meshSH;
    meshSH = meshsrc->GetOutput();

    itkMeshTovtkPolyData * ITKVTKConverter = new itkMeshTovtkPolyData;
    ITKVTKConverter->SetInput( meshSH );

    vtkPolyDataWriter *vtkwriter;
    vtkwriter = vtkPolyDataWriter::New();

    meshWithCurvature = true;
    if(meshWithCurvature)
    {
        MeshType *             paraMesh = meshsrc->GetOutputParaMesh();
        PointsContainerPointer paraPoints = paraMesh->GetPoints();
        int nvert = paraPoints->Size();

        double * curvedness = meshsrc->GetCurvedness();
        vtkDoubleArray* curvature = vtkDoubleArray::New();
        curvature->SetName("Curvature");
//        curvature->SetName("PointScalarData");
        curvature->SetArray(curvedness, nvert, 1);

        Point3 * normal_vector = meshsrc->GetNormalVector();
        vtkDoubleArray* normalVector = vtkDoubleArray::New();
        normalVector->SetName("NormalVector");
        normalVector->SetNumberOfComponents(3);
        for(int i = 0; i < nvert; i++)
          {
          float tuple[3];
          tuple[0] = normal_vector[i][0];
          tuple[1] = normal_vector[i][1];
          tuple[2] = normal_vector[i][2];
          //this will allocate memory as necessary
          normalVector->InsertNextTuple(tuple);
          }

        vtkPolyData* mesh = ITKVTKConverter->GetOutput();
        mesh->GetPointData()->SetScalars(curvature);
        mesh->GetPointData()->SetNormals(normalVector);
        vtkwriter->SetInput(mesh);
    }
    else
    	vtkwriter->SetInput(ITKVTKConverter->GetOutput() );

    outFileName.erase();
    outFileName.append(base_string);
//    outFileName.append("SPHARM.vtk"); // S
    outFileName.append(outSurfFile);

    vtkwriter->SetFileName(outFileName.c_str() );
    vtkwriter->Write();

    if( debug )
      {
      std::cout << "saving par aligned coefs" << std::endl;
      }

    // save coefficients too
    outFileName.erase();
    outFileName.append(base_string);
    outFileName.append("SPHARM.coef");
    typedef neurolib::SphericalHarmonicCoefficientFileWriter CoefWriterType;
    CoefWriterType::Pointer coefwriter = CoefWriterType::New();

    coefwriter->SetFileName(outFileName.c_str() );
    coefwriter->SetInput(coeflist);
    coefwriter->Update();

    if( !NoParaAlignFlag )
      {
      if( debug )
        {
        std::cout << "generating ellipse aligned data" << std::endl;
        }
      // get EllipseAlign
      neurolib::SphericalHarmonicSpatialObject::CoefListType * ellipsoidAlign;
      ellipsoidAlign = spharmFilter->GetEllipseAlignCoef();

      if( debug )
        {
        std::cout << "writing ellipse aligned data" << std::endl;
        }
      neurolib::SphericalHarmonicMeshSource::Pointer meshEllisrc = neurolib::SphericalHarmonicMeshSource::New();
      meshEllisrc->SetCoefs(*ellipsoidAlign);
      meshEllisrc->SetLevel(subdivLevel);
      meshEllisrc->Update();
      MeshType * ellipseMesh = meshEllisrc->GetOutput();

      itkMeshTovtkPolyData * ITKVTKConverter2 = new itkMeshTovtkPolyData;
      ITKVTKConverter2->SetInput(ellipseMesh);

      vtkwriter->SetInput(ITKVTKConverter2->GetOutput() );

      outFileName.erase();
      outFileName.append(base_string);
      outFileName.append("SPHARM_ellalign.vtk");
      vtkwriter->SetFileName(outFileName.c_str() );
//      vtkwriter->Write();

      outFileName.erase();
      outFileName.append(base_string);
      outFileName.append("SPHARM_ellalign.coef");
      coefwriter->SetFileName(outFileName.c_str() );
      coefwriter->SetInput(*ellipsoidAlign);
//      coefwriter->Update();
      }

    if( regTemplateFileOn || regParaTemplateFileOn )
      {
      if( debug )
        {
        std::cout << "writing proc aligned data" << std::endl;
        }
      // load RegTemplateMesh
      MeshType::Pointer RegTemplateMesh = MeshType::New();

      if( regTemplateFileOn )
        {
        VTKreader->SetFileName(regTemplateFile.c_str() );
        VTKreader->Update();

        vtkPolyDataToitkMesh* VTKITKConverter3 = new vtkPolyDataToitkMesh;
        VTKITKConverter3->SetInput(VTKreader->GetOutput() );
        RegTemplateMesh = VTKITKConverter3->GetOutput();

        }

      ProcrustesFilterType::Pointer procrustesFilter = ProcrustesFilterType::New();
      procrustesFilter->SetNumberOfInputs(2);
      if( regTemplateFileOn )
        {
        procrustesFilter->SetInput(0, RegTemplateMesh);
        }
      if( regParaTemplateFileOn )
        {
        procrustesFilter->SetInput(0, regParaTemplateMesh);
        }
      procrustesFilter->SetInput(1, meshSH);
      procrustesFilter->SetUseInitialAverageOff();
      procrustesFilter->SetUseNormalizationOff();
      procrustesFilter->SetUseScalingOff();
      procrustesFilter->SetUseSingleIterationOn();
      procrustesFilter->Update();

      if( procrustesTransformationOutputOn )
        {

        // there are two transforms here
        // but because single iteration is on
        // and useInitialAverage is off
        // the transformation of the second one
        // will be the transformation that maps back to the first one
        // this is what we are interested in here

        typedef itk::TransformFileWriter TransformFileWriterType;
        TransformFileWriterType::Pointer transformFileWriter = TransformFileWriterType::New();
        transformFileWriter->SetInput( procrustesFilter->GetTransform( 1 ) );
        transformFileWriter->SetFileName( procrustesTransformationFile );

        try
          {
          transformFileWriter->Update();
          }
        catch( itk::ExceptionObject & e )
          {
          std::cerr << "Non-fatal exception while trying to write the Procrustes affine transform" << std::endl;
          std::cerr << e << std::endl;
          }

        }

      MeshType::Pointer RegisteredMesh = procrustesFilter->GetOutput(1);

      outFileName.erase();
      outFileName.append(base_string);
      outFileName.append("SPHARM_procalign.vtk");
      std::cout << "procalign" << std::endl;
      itkMeshTovtkPolyData * ITKVTKConverter3 = new itkMeshTovtkPolyData;
      ITKVTKConverter3->SetInput(RegisteredMesh);

      vtkwriter->SetInput(ITKVTKConverter3->GetOutput() );
      vtkwriter->SetFileName(outFileName.c_str() );
      vtkwriter->Write();

      }


    if( writePara )
      {
      if( debug )
        {
        std::cout << "writing para mesh data" << std::endl;
        }
      MeshType *             paraMesh = meshsrc->GetOutputParaMesh();
      PointsContainerPointer paraPoints = paraMesh->GetPoints();

      outFileName.erase();
      outFileName.append(base_string);
      outFileName.append("_para.vtk");

      itkMeshTovtkPolyData * ITKVTKConverter4 = new itkMeshTovtkPolyData;
      ITKVTKConverter4->SetInput(paraMesh);

      vtkwriter->SetInput(ITKVTKConverter4->GetOutput() );
      vtkwriter->SetFileName(outFileName.c_str() );
      vtkwriter->Write();

      // DIMENSION = 1 ; NUMBER_OF_POINTS ; TYPE = Scalar
      // write phi  [0, 2*M_PI]
        {
        outFileName.erase();
        outFileName.append(base_string);
        outFileName.append("_paraPhi.txt");
        std::ofstream efile(outFileName.c_str(), std::ios::out);
        if( !efile )
          {
          std::cerr << "Error: open of file \"" << outFileName  << "\" failed." << std::endl;
          exit(-1);
          }
        efile.precision(10);

        int nvert = paraPoints->Size();
        efile << "NUMBER_OF_POINTS = " << nvert << std::endl;
        efile << "DIMENSION = 1" << std::endl;
        efile << "TYPE = Scalar" << std::endl;
        for( int i = 0; i < nvert; i++ )
          {
          PointType curPoint =  paraPoints->GetElement(i);
          double    phi = atan2(curPoint[1], curPoint[0]) + M_PI; // 0 .. 2 * M_PI
          efile << phi << endl;
          // if (i != nvert - 1) efile << " ";
          }
        efile.close();
        }
        // write phiHalf [0, M_PI]
        {
        outFileName.erase();
        outFileName.append(base_string);
        outFileName.append("_paraPhiHalf.txt");
        std::ofstream efile(outFileName.c_str(), std::ios::out);
        if( !efile )
          {
          std::cerr << "Error: open of file \"" << outFileName  << "\" failed." << std::endl;
          exit(-1);
          }
        efile.precision(10);

        int nvert = paraPoints->Size();
        efile << "NUMBER_OF_POINTS = " << nvert << std::endl;
        efile << "DIMENSION = 1" << std::endl;
        efile << "TYPE = Scalar" << std::endl;
        for( int i = 0; i < nvert; i++ )
          {
          PointType curPoint =  paraPoints->GetElement(i);
          double    phi = atan2(curPoint[1], curPoint[0]) + M_PI;
          if( phi > M_PI ) // convert Pi~2Pi to 0~Pi
            {
            phi = 2 * M_PI - phi;       // 0 .. M_PI .. 0
            }
          efile << phi << endl;
          // if (i != nvert - 1) efile << " ";
          }
        efile.close();
        }

        // write theta
        {
        outFileName.erase();
        outFileName.append(base_string);
        outFileName.append("_paraTheta.txt");
        std::ofstream efile(outFileName.c_str(), std::ios::out);
        if( !efile )
          {
          std::cerr << "Error: open of file \"" << outFileName << "\" failed." << std::endl;
          exit(-1);
          }
        efile.precision(10);

        int nvert = paraPoints->Size();
        efile << "NUMBER_OF_POINTS = " << nvert << std::endl;
        efile << "DIMENSION = 1" << std::endl;
        efile << "TYPE = Scalar" << std::endl;
        for( int i = 0; i < nvert; i++ )
          {
          PointType curPoint =  paraPoints->GetElement(i);
          double    theta = atan(curPoint[2] / sqrt(curPoint[0] * curPoint[0] + curPoint[1] * curPoint[1]) ) + M_PI_2; //0 .. M_PI
          efile << theta << endl;
          // if (i != nvert - 1) efile << " ";
          }
        efile.close();
        }

        {
        // write theta/M_PI * (phi/M_PI/2 + 1)
        outFileName.erase();
        outFileName.append(base_string);
        outFileName.append("_paraMix.txt");
        std::ofstream efile(outFileName.c_str(), std::ios::out);
        if( !efile )
          {
          std::cerr << "Error: open of file \"" << outFileName << "\" failed." << std::endl;
          exit(-1);
          }
        efile.precision(10);

        int nvert = paraPoints->Size();
        efile << "NUMBER_OF_POINTS = " << nvert << std::endl;
        efile << "DIMENSION = 1" << std::endl;
        efile << "TYPE = Scalar" << std::endl;
        for( int i = 0; i < nvert; i++ )
          {
          PointType curPoint =  paraPoints->GetElement(i);
          double    phi = atan2(curPoint[1], curPoint[0]) + M_PI;
          if( phi > M_PI )
            {
            phi = 2 * M_PI - phi;            // 0 .. M_PI ..0
            }
          double theta = atan(curPoint[2] / sqrt(curPoint[0] * curPoint[0] + curPoint[1] * curPoint[1]) ) + M_PI_2;
          // 0 .. M_PI
          efile << theta / M_PI * ( phi / M_PI + 1) << endl;
          // if (i != nvert - 1) efile << " ";
          }
        efile.close();
        }
      }

    bool bullEye=true;
    if(bullEye)
    {
    	// Compute the center of mass
    	vtkSmartPointer < vtkCenterOfMass > centerOfMassFilter = vtkSmartPointer< vtkCenterOfMass > ::New();
    	centerOfMassFilter->SetInput(ITKVTKConverter->GetOutput() );
    	centerOfMassFilter->SetUseScalarsAsWeights(false);
    	centerOfMassFilter->Update();
    	double center1[3];
    	centerOfMassFilter->GetCenter(center1);
    	std::cout << "Mass center is: " << center1[0] << " " << center1[1]<< " " << center1[2] << std::endl;

    	float split1_2 = M_PI*5/3;
    	if(refBasalPoint1||refBasalPoint3)
    	{
    		split1_2 = atan2(-x1Position*inSliceSpacing-center1[0], -y1Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
    		split1_2 = split1_2+M_PI; //in the interval [0,+2pi] radians
    	}
    	std::cout << "split1_2 is: " << split1_2*180/M_PI << " degrees." << std::endl;
    	float split2_3 = split1_2 + M_PI/3;
    	split2_3 = (split2_3<=2*M_PI) ? split2_3 : (split2_3-2*M_PI);
    	float split3_4 = split2_3 + M_PI/3;
    	split3_4 = (split3_4<=2*M_PI) ? split3_4 : (split3_4-2*M_PI);
    	float split4_5 = split3_4 + M_PI/3;
    	split4_5 = (split4_5<=2*M_PI) ? split4_5 : (split4_5-2*M_PI);
    	float split5_6 = split4_5 + M_PI/3;
    	split5_6 = (split5_6<=2*M_PI) ? split5_6 : (split5_6-2*M_PI);
    	float split6_1 = split5_6 + M_PI/3;
    	split6_1 = (split6_1<=2*M_PI) ? split6_1 : (split6_1-2*M_PI);

    	float split7_8 = split1_2;
    	if(refBasalPoint3)
    	{
        	split7_8 = atan2(-x2Position*inSliceSpacing-center1[0], -y2Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
        	split7_8 = split7_8+M_PI; //in the interval [0,+2pi] radians
    	}
    	std::cout << "split7_8 is: " << split7_8*180/M_PI << " degrees." << std::endl;
    	float split8_9 = split7_8 + M_PI/3;
    	split8_9 = (split8_9<=2*M_PI) ? split8_9 : (split8_9-2*M_PI);
    	float split9_10 = split8_9 + M_PI/3;
    	split9_10 = (split9_10<=2*M_PI) ? split9_10 : (split9_10-2*M_PI);
    	float split10_11 = split9_10 + M_PI/3;
    	split10_11 = (split10_11<=2*M_PI) ? split10_11 : (split10_11-2*M_PI);
    	float split11_12 = split10_11 + M_PI/3;
    	split11_12 = (split11_12<=2*M_PI) ? split11_12 : (split11_12-2*M_PI);
    	float split12_7 = split11_12 + M_PI/3;
    	split12_7 = (split12_7<=2*M_PI) ? split12_7 : (split12_7-2*M_PI);

    	float split13_14 = split1_2 + M_PI/12;
    	if(refBasalPoint3)
    	{
        	split13_14 = atan2(-x3Position*inSliceSpacing-center1[0], -y3Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
        	split13_14 = split13_14+M_PI; //in the interval [0,+2pi] radians
    	}
    	std::cout << "split13_14 is: " << split13_14*180/M_PI << " degrees." << std::endl;
    	float split14_15 = split13_14 + M_PI_2;
    	split14_15 = (split14_15<=2*M_PI) ? split14_15 : (split14_15-2*M_PI);
    	float split15_16 = split14_15 + M_PI_2;
    	split15_16 = (split15_16<=2*M_PI) ? split15_16 : (split15_16-2*M_PI);
    	float split16_13 = split15_16 + M_PI_2;
    	split16_13 = (split16_13<=2*M_PI) ? split16_13 : (split16_13-2*M_PI);


        MeshType *             paraMesh = meshsrc->GetOutputParaMesh();
        PointsContainerPointer paraPoints = paraMesh->GetPoints();
        int nvert = paraPoints->Size();
    	double * phi_theta = meshsrc->GetPrecomputedIcosList(); // nvert*2
    	double * phi = new double[nvert];
    	double * theta = new double[nvert];
    	double * curvedness = meshsrc->GetCurvedness();         // nvert*1
    	double curvednessBullsEye[17];
    	for(int i=0; i<17; ++i)
    		curvednessBullsEye[i]=0;

    	int nvertBullEye[17]; // number of vertex in each bull eye
    	for(int i=0; i<17; ++i)
    		nvertBullEye[i]=0;


    	for( int i = 0; i < nvert; i++ )
    	{
    		phi[i] = phi_theta[2 * i]; // 0~2pi
    		theta[i] = phi_theta[2 * i + 1]; // 0~pi


    	if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split6_1<split1_2)&&(phi[i]>=split6_1)&&(phi[i]<split1_2)) || ((split6_1>split1_2)&&((phi[i]>=split6_1)||(phi[i]<split1_2))) ) ) // 0~60;0~60;
    	{
    		curvednessBullsEye[1] += curvedness[i];
    		nvertBullEye[1]++;
    	}
    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split1_2<split2_3)&&(phi[i]>=split1_2)&&(phi[i]<split2_3)) || ((split1_2>split2_3)&&((phi[i]>=split1_2)||(phi[i]<split2_3))) ) ) // 0~60;60~120;
    	{
    		curvednessBullsEye[2] += curvedness[i];
    		nvertBullEye[2]++;
    	}
    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split2_3<split3_4)&&(phi[i]>=split2_3)&&(phi[i]<split3_4)) || ((split2_3>split3_4)&&((phi[i]>=split2_3)||(phi[i]<split3_4))) ) ) // 0~60;120~180;
    	{
    		curvednessBullsEye[3] += curvedness[i];
    		nvertBullEye[3]++;
    	}
    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split3_4<split4_5)&&(phi[i]>=split3_4)&&(phi[i]<split4_5)) || ((split3_4>split4_5)&&((phi[i]>=split3_4)||(phi[i]<split4_5))) ) ) // 0~60;180~240;
    	{
    		curvednessBullsEye[4] += curvedness[i];
    		nvertBullEye[4]++;
    	}
    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split4_5<split5_6)&&(phi[i]>=split4_5)&&(phi[i]<split5_6)) || ((split4_5>split5_6)&&((phi[i]>=split4_5)||(phi[i]<split5_6))) ) ) // 0~60;240~300;
    	{
    		curvednessBullsEye[5] += curvedness[i];
    		nvertBullEye[5]++;
    	}
    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split5_6<split6_1)&&(phi[i]>=split5_6)&&(phi[i]<split6_1)) || ((split5_6>split6_1)&&((phi[i]>=split5_6)||(phi[i]<split6_1))) ) ) // 0~60;300~360;
    	{
    		curvednessBullsEye[6] += curvedness[i];
    		nvertBullEye[6]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split12_7<split7_8)&&(phi[i]>=split12_7)&&(phi[i]<split7_8)) || ((split12_7>split7_8)&&((phi[i]>=split12_7)||(phi[i]<split7_8))) ) ) // 60~120;0~60;
    	{
    		curvednessBullsEye[7] += curvedness[i];
    		nvertBullEye[7]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split7_8<split8_9)&&(phi[i]>=split7_8)&&(phi[i]<split8_9)) || ((split7_8>split8_9)&&((phi[i]>=split7_8)||(phi[i]<split8_9))) ) ) // 60~120;60~120;
    	{
    		curvednessBullsEye[8] += curvedness[i];
    		nvertBullEye[8]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split8_9<split9_10)&&(phi[i]>=split8_9)&&(phi[i]<split9_10)) || ((split8_9>split9_10)&&((phi[i]>=split8_9)||(phi[i]<split9_10))) ) ) // 60~120;120~180;
    	{
    		curvednessBullsEye[9] += curvedness[i];
    		nvertBullEye[9]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split9_10<split10_11)&&(phi[i]>=split9_10)&&(phi[i]<split10_11)) || ((split9_10>split10_11)&&((phi[i]>=split9_10)||(phi[i]<split10_11))) ) ) // 60~120;180~240;
    	{
    		curvednessBullsEye[10] += curvedness[i];
    		nvertBullEye[10]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split10_11<split11_12)&&(phi[i]>=split10_11)&&(phi[i]<split11_12)) || ((split10_11>split11_12)&&((phi[i]>=split10_11)||(phi[i]<split11_12))) ) ) // 60~120;240~300;
    	{
    		curvednessBullsEye[11] += curvedness[i];
    		nvertBullEye[11]++;
    	}
    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split11_12<split12_7)&&(phi[i]>=split11_12)&&(phi[i]<split12_7)) || ((split11_12>split12_7)&&((phi[i]>=split11_12)||(phi[i]<split12_7))) ) ) // 60~120;300~360;
    	{
    		curvednessBullsEye[12] += curvedness[i];
    		nvertBullEye[12]++;
    	}
    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split16_13<split13_14)&&(phi[i]>=split16_13)&&(phi[i]<split13_14)) || ((split16_13>split13_14)&&((phi[i]>=split16_13)||(phi[i]<split13_14))) ) ) // 120~180;-15(345)~75;
    	{
    		curvednessBullsEye[13] += curvedness[i];
    		nvertBullEye[13]++;
    	}
    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split13_14<split14_15)&&(phi[i]>=split13_14)&&(phi[i]<split14_15)) || ((split13_14>split14_15)&&((phi[i]>=split13_14)||(phi[i]<split14_15))) ) ) // 120~180;75~165;
    	{
    		curvednessBullsEye[14] += curvedness[i];
    		nvertBullEye[14]++;
    	}
    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split14_15<split15_16)&&(phi[i]>=split14_15)&&(phi[i]<split15_16)) || ((split14_15>split15_16)&&((phi[i]>=split14_15)||(phi[i]<split15_16))) ) ) // 120~180;165~255;
    	{
    		curvednessBullsEye[15] += curvedness[i];
    		nvertBullEye[15]++;
    	}
    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split15_16<split16_13)&&(phi[i]>=split15_16)&&(phi[i]<split16_13)) || ((split15_16>split16_13)&&((phi[i]>=split15_16)||(phi[i]<split16_13))) ) ) // 120~180;255~345;
    	{
    		curvednessBullsEye[16] += curvedness[i];
    		nvertBullEye[16]++;
    	}
    	else
    	{
    		std::cout << "theta[i] = "<< theta[i] << ";  phi[i] = "<< phi[i] << std::endl;
    		std::cout << "split16_13 = "<< split16_13 << ";  split13_14 = "<< split13_14 << std::endl;
    		std::cout << "split14_15 = "<< split14_15 << ";  split15_16 = "<< split15_16 << std::endl;
    	}
    	}
    	std::cout << std::endl;

        outFileName.erase();
        outFileName.append(bullseyeFile);
        std::ofstream efile(outFileName.c_str(), std::ios::out);
    	   efile.precision(10);
    	   efile << "NUMBER_OF_SEGMENTS = " << 16 << std::endl;

    	for (int i=1; i<=16; ++i)
    	{
    		curvednessBullsEye[i] = curvednessBullsEye[i]/nvertBullEye[i];
    		std::cout << "nvertBullEye["<<i<<"]: " <<nvertBullEye[i]<< ";  curvednessBullsEye["<<i<<"]: "<<curvednessBullsEye[i]<< std::endl;
    		efile << curvednessBullsEye[i] << std::endl;
    	}
    	efile.close();


    }

    }

  catch( itk::ExceptionObject e )
    {
    e.Print(std::cout);
    return EXIT_FAILURE;
    }

  return EXIT_SUCCESS;

}
